#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
from std_msgs.msg import String, Int32

class VoiceCommand:
    def __init__(self):
        # 初始化节点
        rospy.init_node('voice_controller', anonymous=True)
        
        # 创建指令映射字典
        self.command_bindings = {
            'go': 1,
            '前进': 1,
            '前进到抓取位置': 1,
            'catch': 2,
            '抓取': 2,
            '抓': 2,
            'recycle': 3,
            '回收': 3,
            'back': 4,
            '返回': 4,
            '返回到抓取位置': 4,
            'throw': 5,
            '释放': 5,
            '放': 5,
            'color': 6,
            '什么颜色': 6,
            '颜色': 6,
            'stop': 0,
            '停止': 0,
            '暂停': 0
            
        }
        
        # 创建指令发布者
        self.cmd_pub = rospy.Publisher('voice_controller/commands', Int32, queue_size=10)
        
        # 创建系统反馈发布者
        self.feedback_pub = rospy.Publisher('/voice_controller/feedback', String, queue_size=10)
        
        # 订阅语音识别结果
        rospy.Subscriber("/iat_text", String, self.voice_callback)
        
        rospy.loginfo("语音指令解析器已就绪...")
        rospy.spin()

    def voice_callback(self, msg):
        """处理接收到的语音指令"""
        try:
            # 清洗识别结果：转换为小写并去除标点符号
            cmd = msg.data.lower().strip(' 。！？，；：')
            rospy.loginfo(f"原始指令: '{msg.data}' -> 清洗后: '{cmd}'")
            
            # 初始匹配状态为未知
            matched = False
            cmd_code = -1
            
            # 1. 首先尝试精确匹配
            if cmd in self.command_bindings:
                cmd_code = self.command_bindings[cmd]
                rospy.loginfo(f"精确匹配: '{cmd}' -> 指令代码 {cmd_code}")
                matched = True
            
            # 2. 如果没有精确匹配，尝试部分匹配
            if not matched:
                for key, code in self.command_bindings.items():
                    if key in cmd:
                        cmd_code = code
                        rospy.loginfo(f"部分匹配: '{key}' in '{cmd}' -> 指令代码 {cmd_code}")
                        matched = True
                        break
            
            # 3. 如果匹配成功，发布指令
            if matched:
                self.cmd_pub.publish(cmd_code)
                feedback_msg = f"已接受指令: {cmd} (代码:{cmd_code})"
                self.feedback_pub.publish(feedback_msg)
                rospy.loginfo(f"已发布指令代码 {cmd_code} 到 /voice_controller/commands")
            else:
                rospy.logwarn(f"未知指令: '{cmd}'")
                self.feedback_pub.publish(f"未知指令: {cmd}")
                
        except Exception as e:
            rospy.logerr(f"处理指令时出错: {str(e)}")
            self.feedback_pub.publish(f"指令处理错误: {str(e)}")

if __name__ == '__main__':
    try:
        VoiceCommand()
    except rospy.ROSInterruptException:
        pass
